#pragma config(Sensor, in1,    PotArm,              sensorPotentiometer)
#pragma config(Sensor, in2,    DetectTube,          sensorLineFollower)
#pragma config(Sensor, dgtl1,  EncoderR,            sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  EncoderL,            sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  BaseSonar,           sensorSONAR_cm)
#pragma config(Sensor, dgtl7,  Pneu1,            sensorDigitalOut)
#pragma config(Sensor, dgtl11, ArmUp,               sensorTouch)
#pragma config(Sensor, dgtl12, ArmDown,             sensorTouch)
#pragma config(Motor,  port1,           DriveRB,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port2,           DriveRF,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port3,           DriveLF,       tmotorNormal, openLoop)
#pragma config(Motor,  port4,           ArmRL,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port5,           ArmLL,         tmotorNormal, openLoop)
#pragma config(Motor,  port6,           ArmRU,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port7,           ArmLU,         tmotorNormal, openLoop)
#pragma config(Motor,  port8,           SuckR,         tmotorNormal, openLoop)
#pragma config(Motor,  port9,           SuckL,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port10,          DriveLB,       tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
//#include "delta_lib_v3.c" //Main Funtion Library
#include "delta_lib_v3-1.c" //Main Funtion Library
#include "PIDController.c"

#define FULL 127
#define AutonFULL 100

PIDController arm;
PIDController left;
PIDController right;

//---------------------
int k_driveToStackMode 1;
int k_armRaiseMode 3;
int k_driveToGoalMode 4;
int k_dropMode 5;
int k_exhaleReverseMode 7;
int k_finishAutonMode 10;
int k_stackToGoalDist 250;
int k_distToStack 500;
int k_distToGoal = k_distToStack + k_stackToGoalDist;

int currentMode = k_driveToStackMode;

//---------------------------------/ Pre Autonomous /-------------------------------------------//
//--------------------------------/                  /------------------------------------------//
//-------------------------------/                    /-----------------------------------------//

void pre_auton()
{
  //--/ Encoders /-------------------------/
  SensorValue[EncoderL] = 0;
  SensorValue[EncoderR] = 0;

  //--/ Arm Points /-----------------------/
  //goal_value = startpoint + change;
  startpoint = SensorValue[PotArm];  // sets ground point           (0 inches)
  low_descore = startpoint + 1556 - 1236;          // sets low descore arm point  (4.5 inches)
  low_lock = startpoint + 2265 - 1236;             //...lowgoal                   (15 inches)
  high_descore = startpoint + 1879 - 1247;          //...high descore              (x inches)
  high_lock = startpoint + 2599 - 1247;            // ...high goal                (18.5 inches)
  goal_value = high_lock;
  //--/ PID /------------------------------/
  init(arm);
  init(right);
  init(left);

  setPIDs(arm, k_P, k_I, k_D);
  setPIDs(left, k_P, k_I, k_D);
  setPIDs(right, k_P, k_I, k_D);
  setSetpoint(arm, goal_value);
  setSetpoint(left, k_distToStack);
  setSetpoint(right, k_distToStack);
  setMaxError(arm, 100);
  setMaxError(left, 50);
  setMaxError(right, 50);

  enable(arm);
  enable(left);
  enable(right);

  arm.k_P = 3;
  left.k_P = 3;
  right.k_P = 3;
  // All activities that occur before the competition starts
  // Example: clearing encoders, setting servo positions, ...
}

//---------------------------------/ Autonomous /-----------------------------------------------//
//--------------------------------/              /----------------------------------------------//
//-------------------------------/                /---------------------------------------------//

task autonomous()
{
  while(true) {
  switch(currentMode) {
  case 1:
  setSuckSpeed(-FULL);
  if(onTarget(left) == 1 && onTarget(right) == 1) {
  setSuckSpeed(0);
  currentMode = k_armRaiseMode;
  }
  break;
  case 3:
  if(onTarget(arm) == 1) {
  setSetpoint(left, distToGoal);
  setSetpoint(right, distToGoal);
  currentMode = k_driveToGoalMode;
  }
  break;
  case 4:
  if(onTarget(left) == 1 && onTarget(right) == 1) {
  setSetpoint(arm, low_lock);
  currentMode = k_dropMode;
  }
  break;
  case 5:
  setArmSpeed(calculatePID(arm, SensorValue[PotArm]);
  if(onTarget(arm) == 1) {
  setSetpoint(left, distToStack);
  setSetpoint(right, distToStack);
  currentMode = k_exhaleReverseMode;
  }
  break;
  case 7:
  setSuckSpeed(FULL);
  if(onTarget(left) == 1 && onTarget(right) == 1) {
  setSuckSpeed(0);
  currentMode = k_finishAutonMode;
  }
  break;
  case 10:
  setDriveSpeed(0);
  setSuckSpeed(0);
  }
  setArmSpeed(calculatePID(arm, SensorValue[PotArm]);
  setDriveLSpeed(calculatePID(left, SensorValue[EncoderL]));
  setDriveRSpeed(calculatePID(right, SensorValue[EncoderR]));
  }//while
}//task auto

//---------------------------------/ User Control /---------------------------------------------//
//--------------------------------/                /--------------------------------------------//
//-------------------------------/                  /-------------------------------------------//

task usercontrol()
{
  // User control code here, inside the loop
  pre_auton();
  setSetpoint(arm, startpoint);
  while (true)
  {
    //--/ Arm /-------------------------------------/
    if (vexRT[Ch3] > 15 || (vexRT[Ch3]) < -15){
      setArmSpeed(vexRT[Ch3]);
      disable(arm);
      } else {
      setArmSpeed(calculatePID(arm, SensorValue[PotArm]));
    }

    if(vexRT[Btn7U] == 1) {
      setSetPoint(arm, low_lock);
      enable(arm);
      } else if (vexRT[Btn7D] == 1) {
      setSetPoint(arm, low_descore);
      enable(arm);
      } else if(vexRT[Btn6U] == 1) {
      setSetPoint(arm, high_lock);
      enable(arm);
      } else if(vexRT[Btn6D] == 1) {
      setSetPoint(arm, high_descore);
      enable(arm);
      } else if(vexRT[Btn7L] == 1) {
      disable(arm);
    }
    //--/ DESCORE /----------------------------------/

    if(vexRT[Btn7R] == 1) {
      SensorValue[Pneu1] = 1;  // ...activate the solenoid.
    }
    else {
      SensorValue[Pneu1] = 0;  // ..deactivate the solenoid.
    }
    //--/ INHALE /----------------------------------/
    motor[SuckL] = motor[SuckR] = (vexRT[Btn5U] - vexRT[Btn5D]) * FULL;

    //--/ Drive_Train /---------------------------/
    setDriveRSpeed((vexRT[Ch2] - vexRT[Ch1]));// (y + x)
    setDriveLSpeed((vexRT[Ch2] + vexRT[Ch1]));// (y - x)
  }
}
